
public class Love extends Proportional {
	
	public Love() {
		// initializing with 1 to prevent division with 0
		m_highestVal = 1;
		setWaitTime(5);

	}

	public void doWork() throws Exception {
		//initializing controller matrix for agressive behaviour for finding lightbulb
		int[][] k = new int[][] {{0,1,0,1,0,1},{1,0,1,0,1,0}};
		
		//getting sensor values
		int s0 = getLightValue(0);
		int s1 = getLightValue(1);
		int s2 = getLightValue(2);
		int s3 = getLightValue(3);
		int s4 = getLightValue(4);
		int s5 = getLightValue(5);
		
		//getting rough values for left and right motor speed
		int aL = (s0*k[0][0] + s1*k[0][1] + s2*k[0][2] + s3*k[0][3] + s4*k[0][4] + s5*k[0][5]);
		int aR = (s0*k[1][0] + s1*k[1][1] + s2*k[1][2] + s3*k[1][3] + s4*k[1][4] + s5*k[1][5]);
		
		//Searching for biggest speed value
		if (aL > m_highestVal) {m_highestVal = aL;}
		if (aR > m_highestVal) {m_highestVal = aR;}
			
		//scaling the speed value between 0 and 5
		int speedL = (5 * aL) / m_highestVal;
		int speedR = (5 * aR) / m_highestVal;
		
		setMotorSpeeds(speedL, speedR);
		
		// while lightbulb is too close, drive back
		if (aL < 800 || aR < 800) {
			aL = 1 -(s0*k[0][0] + s1*k[0][1] + s2*k[0][2] + s3*k[0][3] + s4*k[0][4] + s5*k[0][5]);
			aR = 1 - (s0*k[1][0] + s1*k[1][1] + s2*k[1][2] + s3*k[1][3] + s4*k[1][4] + s5*k[1][5]);
			speedL = (5 * aL) / m_highestVal;
			speedR = (5 * aR) / m_highestVal;			
			setMotorSpeeds(speedL, speedR);
		}
		//if no lightbulb is in reach (high sensor value) avoid wall contact
		if ((aL + aR)/2 > 1300) {
			handleWallContact();
		}

	}


	public void close() throws Exception {
	}

}